//
// Created by seeed on 25-8-14.
//

#include <Arduino.h>
#include "myTask.h"
#include <cstdio>
#include "global/GObject.h"
// 引入模型元数据以获取期望输入尺寸
#include "../../lib/SolderingIron_inferencing/src/model-parameters/model_metadata.h"

#if DISABLE_CAMERA_TASK == 0
// 图像缓冲区（摄像头原始尺寸 160x120 RGB888）
uint8_t *image_buf = nullptr;
const int IMAGE_BUF_SIZE = EI_CAMERA_RAW_FRAME_BUFFER_COLS * EI_CAMERA_RAW_FRAME_BUFFER_ROWS * 3;

// 经过调整后的模型输入图像缓冲（96x96 RGB888）
static uint8_t *resized_rgb_buf = nullptr;
static const int MODEL_INPUT_WIDTH = EI_CLASSIFIER_INPUT_WIDTH;
static const int MODEL_INPUT_HEIGHT = EI_CLASSIFIER_INPUT_HEIGHT;
static const int MODEL_INPUT_SIZE_BYTES = MODEL_INPUT_WIDTH * MODEL_INPUT_HEIGHT * 3; // RGB888

// 模型输入特征缓冲（以 float 承载，但值范围保持在 0..255，交由量化模型处理）
static float *feature_buf = nullptr;
static size_t feature_buf_size = 0;
#endif

// 任务句柄
TaskHandle_t LED_Task_Handle = NULL;    // LED任务
TaskHandle_t Camera_Task_Handle = NULL; // 相机任务
TaskHandle_t Monitor_Task_Handle = NULL; // 开关检测任务


// LED任务
[[noreturn]] void LED_Task_Func(void *pvParameters) {
    printf("LED_Task start running...\n");
    while (true) {
        if (nowRunMode == AI_MODE) {    // AI模式下LED 呼吸灯
            led0.breathe();
        }else{                          // 手动模式下LED常亮
            led0.on();
            vTaskDelay(200 / portTICK_PERIOD_MS);
        }
    }
}

// 相机任务
[[noreturn]] void Camera_Task_Func(void *pvParameters) {
#if DISABLE_CAMERA_TASK == 1
    printf("Camera_Task disabled\n");
    vTaskDelete(NULL); // 删除当前任务
#else
    printf("Camera_Task start running...\n");
    
    // 设定分辨率为 160x120，避免后续缩放
    if (!camera0.setFrameSize(FRAMESIZE_QQVGA)) {
        printf("相机设置分辨率失败，使用默认分辨率\n");
    }
    
    printf("相机初始化成功\n");
    
    while (true) {
        printf("开始捕获并推理...\n");

        // 直接获取帧缓冲
        camera_fb_t *fb = camera0.capture();
        if (!fb) {
            printf("图像捕获失败\n");
            vTaskDelay(500 / portTICK_PERIOD_MS);
            continue;
        }

        // 将JPEG转换为RGB888到 image_buf
        bool rgb_ok = true;
        if (fb->format == PIXFORMAT_JPEG) {
            rgb_ok = camera0.jpegToRGB888(fb, image_buf);
        } else if (fb->format == PIXFORMAT_RGB888) {
            size_t copy_len = (size_t)fb->width * fb->height * 3;
            if (copy_len > (size_t)IMAGE_BUF_SIZE) copy_len = IMAGE_BUF_SIZE;
            memcpy(image_buf, fb->buf, copy_len);
        } else if (fb->format == PIXFORMAT_RGB565) {
            // 简单RGB565转RGB888（如遇此格式）
            uint16_t *src = (uint16_t*)fb->buf;
            for (int i = 0; i < EI_CAMERA_RAW_FRAME_BUFFER_COLS * EI_CAMERA_RAW_FRAME_BUFFER_ROWS; ++i) {
                uint16_t p = src[i];
                uint8_t r = ((p >> 11) & 0x1F) << 3;
                uint8_t g = ((p >> 5) & 0x3F) << 2;
                uint8_t b = (p & 0x1F) << 3;
                image_buf[i * 3 + 0] = r;
                image_buf[i * 3 + 1] = g;
                image_buf[i * 3 + 2] = b;
            }
        } else {
            rgb_ok = false;
        }

        if (!rgb_ok) {
            printf("图像转换失败\n");
            esp_camera_fb_return(fb);
            vTaskDelay(500 / portTICK_PERIOD_MS);
            continue;
        }

        // 准备模型输入：将 160x120 RGB888 调整为 96x96 RGB888，然后拷贝到特征缓冲
        if (feature_buf && resized_rgb_buf && feature_buf_size >= (size_t)EI_CLASSIFIER_NN_INPUT_FRAME_SIZE) {
            // 调整尺寸到模型期望大小
            bool resize_ok = camera0.resizeImage(
                image_buf,
                EI_CAMERA_RAW_FRAME_BUFFER_COLS,
                EI_CAMERA_RAW_FRAME_BUFFER_ROWS,
                resized_rgb_buf,
                MODEL_INPUT_WIDTH,
                MODEL_INPUT_HEIGHT
            );
            if (!resize_ok) {
                printf("图像尺寸调整失败\n");
                esp_camera_fb_return(fb);
                vTaskDelay(500 / portTICK_PERIOD_MS);
                continue;
            }

            // 将 uint8 RGB 数据复制到 float 特征缓冲，保持 0..255，不做 0..1 归一化
            // 模型为 INT8 量化，Edge Impulse 内部完成量化处理
            for (size_t i = 0; i < (size_t)MODEL_INPUT_SIZE_BYTES; ++i) {
                feature_buf[i] = (float)resized_rgb_buf[i];
            }
            int pred_label = -1;
            float pred_score = 0.0f;
            int rc = model0.runClassifier(
                feature_buf,
                (size_t)EI_CLASSIFIER_NN_INPUT_FRAME_SIZE,
                &pred_label,
                &pred_score,
                false
            );
            if (rc != 0) {
                printf("推理失败: %d\n", rc);
            } else {
                // 打印推理结果（Python脚本会在控制台显示文本）
                const char *label_str = model0.getLabel(pred_label);
                if (!label_str) label_str = "<null>";
                Serial.printf("RESULT label=%s index=%d score=%.5f\n", label_str, pred_label, pred_score);
            }
        } else {
            printf("模型输入缓冲未就绪或尺寸不足\n");
        }

        // 通过串口发送JPEG数据，供 display_image.py 显示
        if (fb->format == PIXFORMAT_JPEG) {
            Serial.write(fb->buf, fb->len);
        } else {
            // 若不是JPEG，尽量转换后再发送（此处简单跳过）
            // 可扩展：将 RGB888 再编码为 JPEG 后发送
        }

        // 打印图像元信息（便于调试）
        Serial.printf("\n图像信息: 宽度=%d, 高度=%d, 大小=%d字节, 格式=%s\n",
                      fb->width, fb->height, fb->len,
                      fb->format == PIXFORMAT_JPEG ? "JPEG" :
                      fb->format == PIXFORMAT_RGB565 ? "RGB565" :
                      fb->format == PIXFORMAT_RGB888 ? "RGB888" :
                      fb->format == PIXFORMAT_GRAYSCALE ? "GRAYSCALE" : "UNKNOWN");

        // 归还帧缓冲
        esp_camera_fb_return(fb);

        // 间隔
        vTaskDelay(1000 / portTICK_PERIOD_MS);
    }
#endif
}


//相机魔像初始化
void Camera_Init() {
#if DISABLE_CAMERA_TASK == 0
    // 分配图像缓冲区
    image_buf = (uint8_t*)calloc(IMAGE_BUF_SIZE, sizeof(uint8_t));
    if (image_buf == nullptr) {
        printf("图像缓冲区内存分配失败\n");
    }
    // 分配调整后 RGB 图像缓冲区（匹配模型输入尺寸）
    resized_rgb_buf = (uint8_t*)calloc(MODEL_INPUT_SIZE_BYTES, sizeof(uint8_t));
    if (resized_rgb_buf == nullptr) {
        printf("模型输入图像缓冲区内存分配失败\n");
    }

    // 分配并初始化模型输入特征缓冲区（严格匹配模型期望特征长度）
    feature_buf_size = (size_t)EI_CLASSIFIER_NN_INPUT_FRAME_SIZE;
    feature_buf = (float*)calloc(feature_buf_size, sizeof(float));
    if (feature_buf == nullptr) {
        printf("模型输入缓冲区内存分配失败\n");
    }
#endif

}

// 监测任务
[[noreturn]] void Monitor_Task_Func(void *pvParameters) {
    bool lastButtonState = false; // 记录上一次按钮状态
    while (true) {
        if (nowRunMode == AI_MODE) {    // AI模式
            // 读取AI引脚
            if (digitalRead(AIResult_Pin) == HIGH) {    // 识别到人脸
                relay0.on() ;   // 打开继电器
                Serial.println("识别到人脸");
            } else {
                relay0.off() ;  // 关闭继电器
                Serial.println("未识别到人脸");
            }
            vTaskDelay(2000 / portTICK_PERIOD_MS);
        }else{                          // 手动模式
            bool currentButtonState = button0.isPressed();
            
            // 检测按钮状态变化：按下并释放才算一次有效操作
            if (!currentButtonState && lastButtonState) {
                Serial.println("按键按下并释放");
                relay0.toggle();   // 翻转继电器
                vTaskDelay(500 / portTICK_PERIOD_MS);   // 防止重复触发
            }
            
            lastButtonState = currentButtonState;
        }
        vTaskDelay(100 / portTICK_PERIOD_MS);
    }
}


// 任务初始化
void Task_init() {
    Camera_Init();  // 相机初始化

    // 创建LED任务（参数：任务函数，任务名，栈大小，参数，优先级，任务句柄）
    xTaskCreate(LED_Task_Func, "LED_Task", 2048, NULL, 1, &LED_Task_Handle);

    // 创建监测任务
    xTaskCreate(Monitor_Task_Func, "Monitor_Task", 2048, NULL, 1, &Monitor_Task_Handle);

#if DISABLE_CAMERA_TASK == 0
    // 创建相机任务
    xTaskCreate(Camera_Task_Func, "Camera_Task", 6096, NULL, 2, &Camera_Task_Handle);
#endif
}

extern "C" void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) {
    Serial.printf("\n!!! 严重错误: 栈溢出 !!!\n");
    Serial.printf("任务: %s\n", pcTaskName);

    // 死循环 - 需要手动复位
    while(true) {
        delay(1000);
        Serial.println("系统挂起，请复位设备");
    }
}